#include "world.h"

using namespace TorPhysix;

World::World()
{
	firstReg = NULL;
	contacts.resize(8);
	cData.contactArray = &contacts[0];
	resolver.setIterations(64, 42);
}

void World::startFrame()
{
	// Make sure there is room for a reasonable amount of contacts
	contacts.resize( physObjList.size() * 2 );

	BodyRegistration *reg = firstReg;
	while(reg)
	{
		reg->body->clearAccumulators();
		reg->body->calculateDerivedData();

		// get the next registration
		reg = reg->next;
	}
}

void World::generateContacts()
{
	// Setup contacts
	cData.reset( contacts.size() );
	cData.friction = (real)0.9f; //.6
	//cData.restitution = (real)0.6;
	cData.restitution = (real)0.6;
	cData.tolerance = (real) 0.1f;
	//cData.tolerance = (real)0.1;

	for(size_t i=0; i<physObjList.size(); i++)
	{
		for(size_t j=i+1; j<physObjList.size(); j++)
		{
			physObjList[i]->acceptVisitor( *(physObjList[j]), &cData );
		}
	}

}

void World::runPhysics(real dt)
{
	int numIters = 5;
	real delta = (real) (dt * 1.0 / real(numIters) );
	for ( int iters =0; iters < numIters; iters++ )
	{
		for(size_t i=0; i<physObjList.size(); i++)
		{
			physObjList[i]->collisionPrimitive->body->integrate(delta);
			physObjList[i]->collisionPrimitive->calculateInternals();
		}


		contacts.resize( 1 + physObjList.size() * 2 );
		cData.contactArray = &contacts[0];
		cData.contactCount = contacts.size();
		generateContacts();
		performance.contactsGeneratedLast = cData.contactCount;
		performance.maxContacts = contacts.size();
		resolver.resolveContacts( cData.contactArray, cData.contactCount, delta );

		performance.posItersMax = resolver.positionIterations;
		performance.posItersUsed = resolver.positionIterationsUsed;
		performance.velItersMax = resolver.velocityIterations;
		performance.velItersUsed = resolver.velocityIterationsUsed;
	}


	// Apply force generators
	//registry.updateForces(dt);

	// Now, integrate each body
	//BodyRegistration *reg = firstReg;

	//while(reg)
	//{
	//	reg->body->integrate(dt);

	//	// get the next registration
	//	reg = reg->next;
	//}
}

void World::addBody( RigidBody *newBody )
{
	if( firstReg == NULL )
	{
		firstReg = new BodyRegistration;
		firstReg->body = newBody;
		firstReg->next = NULL;
	}else
	{
		BodyRegistration *reg = firstReg;
		while(reg->next)
		{
			reg = reg->next;
		}
		BodyRegistration *newReg = new BodyRegistration;
		newReg->body = newBody;
		newReg->next = NULL;
		// Add to list
		reg->next = newReg;
	}
}


void TorPhysix::World::addPhysicsObject( PhysicsObject *newPhysObj )
{
	physObjList.push_back( newPhysObj );
	addBody( newPhysObj->collisionPrimitive->body );
}

void TorPhysix::World::clear()
{
	physObjList.clear();
	firstReg = NULL;
}
